package anyVehicle;

import java.security.acl.LastOwnerException;
import java.util.HashSet;
import java.util.Set;

import org.lwjgl.util.vector.Matrix4f;
import org.lwjgl.util.vector.Vector3f;
import org.lwjgl.util.vector.Vector4f;

import net.minecraft.src.AxisAlignedBB;
import net.minecraft.src.Entity;
import net.minecraft.src.Vec3D;
import net.minecraft.src.World;

public class VehicleUtil {

	
	private static double dirs[][];

	static {
    	dirs = new double[][] {
    			{ -1, 0, 0 },
    			{ 1, 0, 0 },
    			{ 0, -1, 0},
    			{ 0, 1, 0},
    			{ 0, 0, -1},
    			{ 0, 0, 1}
    	};

	}

	public static void drawCube(World world, int x, int y, int z, int x0, int y0, int z0, Set<Integer> immutableBlockIDs, int bid) {
		line(world, x, y, z, x, y0, z, bid, immutableBlockIDs);
		line(world, x0, y, z0, x0, y0, z0, bid, immutableBlockIDs);
		line(world, x, y, z0, x, y0, z0, bid, immutableBlockIDs);
		line(world, x0, y, z, x0, y0, z, bid, immutableBlockIDs);
		
		line(world, x, y, z, x0, y, z, bid, immutableBlockIDs);
		line(world, x, y0, z, x0, y0, z, bid, immutableBlockIDs);
		line(world, x, y, z0, x0, y, z0, bid, immutableBlockIDs);
		line(world, x, y0, z0, x0, y0, z0, bid, immutableBlockIDs);
		
		line(world, x, y, z, x, y, z0,  bid, immutableBlockIDs);
		line(world, x0, y, z, x0, y, z0,  bid, immutableBlockIDs);
		line(world, x, y0, z, x, y0, z0,  bid, immutableBlockIDs);
		line(world, x0, y0, z, x0, y0, z0,  bid, immutableBlockIDs);
	}


	public static void line(World world, int x1, int y1, int z1, int x2, int y2, int z2, int blockID, Set<Integer> immutableBlockIDs) {
		if (immutableBlockIDs == null) {
			immutableBlockIDs = new HashSet<Integer>();
		}
		Vec3D step = Vec3D.createVector(x2-x1, y2-y1, z2-z1).normalize();
		double stepX = step.xCoord;
		double stepY = step.yCoord;
		double stepZ = step.zCoord;
		
		double posX = x1;
		double posY = y1;
		double posZ = z1;
		
		int roundPosX = (int) Math.round(posX);
		int roundPosY = (int) Math.round(posY);
		int roundPosZ = (int) Math.round(posZ);
		while (x2 != roundPosX || y2 != roundPosY || z2 != roundPosZ) {
			if (!immutableBlockIDs.contains(world.getBlockId(roundPosX, roundPosY, roundPosZ))) {
				world.setBlockWithNotify(roundPosX, roundPosY, roundPosZ, blockID);
			}
			posX +=stepX;
			posY +=stepY;
			posZ +=stepZ;
			
			roundPosX = (int) Math.round(posX);
			roundPosY = (int) Math.round(posY);
			roundPosZ = (int) Math.round(posZ);

		}
	}
	
	public static void createCube(World world, int x, int y, int z, int length, int height, int width, int blockID) {
		for (int i=x; i<x+length; i++) {
			for (int j=y; j<y+height; j++) {
				for (int k=z; k<z+width; k++) {
					world.setBlock(i, j, k, blockID);
				}
			}
		}
		for (int i=x; i<x+length; i++) {
			for (int j=y; j<y+height; j++) {
				for (int k=z; k<z+width; k++) {
					world.markBlockNeedsUpdate(i, j, k);
			        world.notifyBlocksOfNeighborChange(i, j, k, blockID);				}
			}
		}
	}


	/**
	 * @param localPos The coordinates of the object in  local coordinates
	 * @param pos The pos of the origin of the local coord system in global coordinates
	 * @param rot The rotation of the local coord system in global coordinates (roll, yaw, pitch)
	 * @return
	 */
	public static Vector4f localPosToParent(Vector4f localPos, Vector3f pos, Vector3f rot) {
		Matrix4f m = new Matrix4f();
		m.translate(pos);
		rotate(m, rot);
		Vector4f result = new Vector4f();
		Matrix4f.transform(m, localPos, result);
		return result;
	
	}

	public static Vector4f localVelToParent(Vector4f localVel, Vector3f rot) {
		Matrix4f m = new Matrix4f();
		rotate(m, rot);
		Vector4f result = new Vector4f();
		Matrix4f.transform(m, localVel, result);
		return result;
	}
	
	/**
	 * @param parentPos Position of the object in parent coordinates
	 * @param pos The pos of the origin of the local coord system in global coordinates
	 * @param rot The rotation of the local coord system in global coordinates (roll, yaw, pitch)
	 * @return
	 */
	public static Vector4f parentPosToLocal(Vector4f parentPos, Vector3f pos, Vector3f rot) {
		Matrix4f m = new Matrix4f();
		rotateInverse(m, rot);
		m.translate(pos.negate(pos));
	
		Vector4f result = new Vector4f();
		Matrix4f.transform(m, parentPos, result);
		return result;
	
	}

	public static Vector4f parentVelToLocal(Vector4f parentVel, Vector3f rot) {
		Matrix4f m = new Matrix4f();
		rotateInverse(m, rot);
	
		Vector4f result = new Vector4f();
		Matrix4f.transform(m, parentVel, result);
		return result;
	}


//	/**
//	 * roll = atan2(m21, m11)
//	 * yaw = atan2(m02, m00)
//	 * pitch = asin(-m01)
//	 * @param localRot Rotation in local coordinates
//	 * @param rot Rotation of the local coordinate system in global coordinates
//	 * @return
//	 */
//	public static Vector3f localRotToParent(Vector3f localRot, Vector3f rot) {
//		Matrix4f m = new Matrix4f();
//		System.out.println("Rot: " + rot);
//		rotate(m, rot);
//		System.out.println("LocalRot: " + localRot);
//		rotate(m, localRot);
//		
//		Vector3f result = getEulerAnglesFromMatrix(m);
//		
//		return result;
//	
//	}
//
//
//
//
//	public static Vector3f parentRotToLocal(Vector3f parentRot, Vector3f rot) {
//		Matrix4f m = new Matrix4f();
//		rotate(m, parentRot);
//		rotateInverse(m, rot);
//		
//		Vector3f result = getEulerAnglesFromMatrix(m);
//		
//		return result;
//	
//	}


	public static void rotate(Matrix4f m, Vector3f rot) {
		m.rotate(rot.y, new Vector3f(0F, 1F, 0F));
		m.rotate(rot.z, new Vector3f(0F, 0F, 1F));
		m.rotate(rot.x, new Vector3f(1F, 0F, 0F));
	}
	
	public static void rotateInverse(Matrix4f m, Vector3f rot) {
		m.rotate(-rot.x, new Vector3f(1F, 0F, 0F));
		m.rotate(-rot.z, new Vector3f(0F, 0F, 1F));
		m.rotate(-rot.y, new Vector3f(0F, 1F, 0F));
	}
	
	public static Vector3f getEulerAnglesFromMatrix(Matrix4f m) {
		Vector3f result = new Vector3f(
				(float)Math.atan2(m.m21, m.m11),
				-(float)Math.atan2(m.m02, m.m00),
				-(float)Math.asin(-m.m01)
				);
		return result;
	}
	
	public static void main (String args[]) {
		Vector3f pos = new Vector3f(10, 0, 10);
		Vector4f localCoords = new Vector4f(10,10,10,1);
		Vector3f localRot = new Vector3f( (float) (45F/180F * Math.PI), (float) (45F/180F * Math.PI), (float) (45F/180F * Math.PI));
		Vector3f rot = new Vector3f((float) (2 * Math.PI) , (float) (1.0 * Math.PI), (float) (1.0 * Math.PI));

		Vector4f parentCoords = localPosToParent(localCoords, pos, rot);
		System.out.println(parentCoords);
		System.out.println(parentPosToLocal(parentCoords, pos, rot));
		
//		Vector3f parentRot = localRotToParent(localRot, rot);
//		System.out.println(parentRot);
//		System.out.println(parentRotToLocal(parentRot, rot));
	}


	public static int findWaterLevel(World worldObj, int i, int j, int k, int lengthBlocks, int heightBlocks, int widthBlocks) {
		int y=j+heightBlocks-1;
		for (; y>=j; y--) {
			for (int x=i; x<i+lengthBlocks; x++) {
				for (int z=k; z<k+widthBlocks; z++) {
					if (worldObj.getBlockId(x, y, z) == 8 || worldObj.getBlockId(x, y, z) == 9) { // finds water
						return y;
					}
				}
			}
		}
		return y;
	}


	public static Vector3f getPosVector3f(Entity entity) {
		return new Vector3f((float) entity.posX, (float) entity.posY, (float) entity.posZ);
	}


	public static Vector4f getPosVector4f(Entity entity) {
		return new Vector4f((float) entity.posX, (float) entity.posY, (float) entity.posZ, 1F);
	}

	public static Vector4f getVelVector4f(Entity entity) {
		return new Vector4f((float) entity.motionX, (float) entity.motionY, (float) entity.motionZ, 1F);
	}


	public static Vector3f getRotVector3f(Entity entity) {
		return new Vector3f(0F, (float)Math.toRadians(entity.rotationYaw), (float)Math.toRadians(entity.rotationPitch));
	}


	public static Vector3f getLastTickPosVector3f(Entity vehicle) {
		return new Vector3f((float)vehicle.lastTickPosX, (float)vehicle.lastTickPosY,(float) vehicle.lastTickPosZ);
	}

	public static Vector4f getLastTickPosVector4f(Entity parentEntity) {
		return new Vector4f((float)parentEntity.lastTickPosX, (float)parentEntity.lastTickPosY,(float) parentEntity.lastTickPosZ, 1F);
	}
	
	public static javax.vecmath.Vector3f getHalfSizeVector3f(Entity entity) {
		return new javax.vecmath.Vector3f((float)entity.width /2f, (float)entity.height/2f, (float)entity.width/2f);
	}


	public static double returnOrigIfDifferenceTooLow(double orig, double other) {
		if (Math.abs(orig - other) < 1E-4) {
			return orig;
		} else {
			return other;
		}
	}
	

    public static Vec3D collideWithBlock(AxisAlignedBB entityBB, Vec3D entityV, AxisAlignedBB bb)
    {
    	
    	double dist[] = new double[6];
    	dist[0] = entityBB.maxX - bb.minX;
    	dist[1] = bb.maxX - entityBB.minX;
    	dist[2] = entityBB.maxY - bb.minY;
    	dist[3] = bb.maxY - entityBB.minY ;
    	dist[4] = entityBB.maxZ - bb.minZ;
    	dist[5] = bb.maxZ - entityBB.minZ;

    	if (Math.abs(entityV.yCoord) > 0.4) {
    		System.out.printf("%.2f, %.2f, %.2f, %.2f, %.2f, %.2f\n", dist[0], dist[1], dist[2], dist[3], dist[4], dist[5]);
    	}
    	
//    	if (Math.abs(entityV.yCoord) > Math.abs(entityV.zCoord) &&  Math.abs(entityV.yCoord) > Math.abs(entityV.xCoord)) {
//    		if (entityV.yCoord < 0) {
//    			return Vec3D.createVector(0, dist[2], 0);
//    		} else {
//    			return Vec3D.createVector(0, -dist[3], 0);
//    		}
//    	}
    	
//		if (entityV.yCoord <= -0.95) {
//			return Vec3D.createVector(0, dist[2], 0);
//		}
//		
//		if (entityV.yCoord >= 0.95) {
//			return Vec3D.createVector(0, -dist[3], 0);
//		}
    	
		if (entityV.yCoord < 0 && dist[3] >= 1.0) {
			return Vec3D.createVector(0, dist[2], 0);
		}
		
		if (entityV.yCoord > 0 && dist[2] >= 1.0) {
			return Vec3D.createVector(0, -dist[3], 0);
		}

    	double minDist=Double.MAX_VALUE;
    	int minPos = 0;
    	for (int i=0; i<dist.length; i++) {
    		if (dist[i] < minDist && dist[i] > 0D) {
    			minDist = dist[i];
    			minPos = i;
    		}
    	}
    	
    	
    	return Vec3D.createVector(dist[minPos] * dirs[minPos][0], dist[minPos] * dirs[minPos][1], dist[minPos] * dirs[minPos][2]);
    	
    }

	public static double assignIfDifferenceIsGreaterThan(double difference, float newValue, double oldValue) {
		if (Math.abs(newValue - oldValue) > difference) {
			return newValue;
		} else {
			return oldValue;
		}
	}


}
